#include <sstream>
#include <iostream>
#include <string>
#include <cstring>
#include <math.h>
#include <stdio.h>



#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <webots/Robot.hpp>

#include "RWLaserServo.h"

using namespace std;
using namespace webots;




RWLaserServo::RWLaserServo()
{

}
RWLaserServo::~RWLaserServo()
{

}

void RWLaserServo::init(webots::Robot* R, ros::NodeHandle* n_in,string name)
{
  n = n_in;
  time_step = R->getBasicTimeStep();
  v_res = 1;
  v_max = 45;
  v_min = -45;
  m_pi = acos(0)*2;
  position = v_max*m_pi/180;
  n_iter = 0;
  pt_sign = -1;
  //LIDAR
  LMS = R->getCamera(name);
  LMS->enable(time_step);
  lms_width = LMS->getWidth();
  lms_height = (v_max-v_min)/v_res;
  //SERVO
  stringstream ss;
  ss << name << "_servo";
  PT_SERVO = R->getServo(ss.str());
  PT_SERVO->enablePosition(time_step);

  //ROS MESSAGE

  LidarScan.angle_min = (float)(-m_pi/2);
  LidarScan.angle_max = (float)(m_pi/2);
  LidarScan.angle_increment = (float)(m_pi/lms_width);
  LidarScan.range_min = LMS->getNear();
  LidarScan.range_max = LMS->getMaxRange();
  LidarScan.ranges.reserve(180);
  LidarScan.header.seq = 0;

  //ROS PUBLISHER
  std::stringstream RPubname,RFramename;
  RPubname << R->getName() << "/laserscan";
  RFramename << "LaserScan";
  pub = n->advertise<sensor_msgs::LaserScan>(RPubname.str(),1000);
  LidarScan.header.frame_id = RFramename.str();

  //Set servo in max position
  PT_SERVO->setPosition(position);
  do{
    R->step(time_step);
    position = PT_SERVO->getPosition();
  }while(fabs(position-(v_max*m_pi/180))>0.005);
  v_count = 0;
}

sensor_msgs::LaserScan RWLaserServo::getScan()
{
  if (n_iter==0)
    cout<< "Scan is empty"<<endl;

    return LidarScan;
}

void RWLaserServo::iterate()
{
  n_iter++;
  float distance;
  lms_values = (float*)LMS->getImage();
  for(int i=0;i<lms_width;i++)
  {
    distance = lms_values[i];
    if (distance > LidarScan.range_max-0.1)
      distance = LidarScan.range_max;
    LidarScan.ranges[i] = distance;
  }
  LidarScan.header.seq++;
  LidarScan.header.stamp = ros::Time::now();
  pub.publish(LidarScan);
  if(pt_sign == -1)
  {
    if (v_count >= lms_height)
    {
      pt_sign = 1;
      v_count--;
    }
    else
      v_count++;
  }
  else if (pt_sign == 1)
  {
    if (v_count < 0)
    {
      pt_sign = -1;
      v_count++;
    }
    else
      v_count--;
  }
  position = (v_max*m_pi/180)-v_count*(v_res*m_pi/180);
  PT_SERVO->setPosition(position);

}

